#include "../include/unrealmap.h"

using Glib::Mutex;
UnrealPlace::UnrealPlace(char* str){
	parse(str);
}
UnrealPlace::UnrealPlace(string str){
	parse(str);
}
void UnrealPlace::parse(string str){
	char cstr[str.size()];
	strcpy(cstr, str.c_str());
	parse(cstr);
}
void UnrealPlace::parse(char* str){
    //parse from comma sep. value
    char* iterIn = str;
    char* iterOut= str;
    unsigned positions[3];
    int curpos=1;
    positions[0]=(unsigned) str;
    for (;*iterIn; iterIn++){
        if (*iterIn == '.')
            continue;
        if (*iterIn == ',')
            positions[curpos++]=(unsigned) iterOut+1;
        *iterOut++=*iterIn;
    }
    *iterOut=0;

    x=atoi((char*) positions[0]);
    y=atoi((char*) positions[1]);
    z=atoi((char*) positions[2]);
}

 double UnrealPlace::lengthSquare(Place* to){
	double twoDLen= Place::lengthSquare(to);
	if (twoDLen==INT_MAX){
		return INT_MAX;
	}
    return (double) ( ABSDIF( ((UnrealPlace*)to)->z, z)^2) + twoDLen;
}



UnrealMap::UnrealMap():RouteMap()//, nodes(1000)
{
    //ctor
    zMax=INT_MIN;
    zMin=INT_MAX;

}
/** readinput
 *should be called after the SNAV command has been received.
 *read out up to and including the ENAV command and iserts al the navpoint in the database.
*/

void UnrealMap::readInput(UnrealStream& input){
    Mutex::Lock lock(mutex);
    string line;
    Node* newNode = 0;
newLine:
    line = input.nextMessage();
//    cout << line <<endl;
    if (line=="SNGP"){
        readInputSNGP(input, newNode);
        goto newLine;
    }
    else if(line=="NAV" || line=="ENAV"){
        if(newNode){
            nodes.push_back(*newNode);
        #ifdef _DEBUG
            cout << "Node added:" << ((UnrealNodeId*) newNode->id)->id;
            cout << " in " << ((UnrealNodeId*) newNode->id)->category;
            cout << "    at: (" << newNode->place->x << "x" << newNode->place->y <<")\n";
            cout.flush();
        #endif
            if(newNode->id->id >= prefixCounts[((UnrealNodeId*) newNode->id)->category]){
                cout << "increased cat " << ((UnrealNodeId*) newNode->id)->category << "to " << newNode->id->id << endl;
                cout.flush();
                prefixCounts[((UnrealNodeId*) newNode->id)->category] = newNode->id->id + 1;
            }
        #ifdef _DEBUG
            cout << "deleting(" << newNode << ")...";
            cout.flush();
        #endif
//            delete newNode;
#ifdef _DEBUG
            cout << "deleted\n";
            cout.flush();
#endif
            newNode=0;
        }
        cout << "creating new...";
        cout.flush();
        newNode = createNode(line);
        cout << "created\n";
        cout.flush();

        for (int i=0; i<input.fieldCount();i++){
            string field = input[i].first;
            string value = input[i].second;
            cout << field << ": " << value << endl;

            if(field=="Id"){
                UnrealNodeId* newId = new UnrealNodeId(0, 0);
                int numPos = value.find_last_not_of("0123456789")+1;
                newId->id = atoi(value.c_str()+numPos);
                value.resize(numPos);
                cout << "id:" << newId->id << endl;
                cout.flush();
//                string* pref;
                vector<string>::iterator pref = find(prefixes.begin(), prefixes.end(), value);
                if(prefixes.size()==0 || pref==prefixes.end()){
                    prefixes.push_back(value);
                    prefixCounts.push_back(newId->id);
                    pref=prefixes.end()-1;

                    cout << "############################ new category:" << value <<endl;
                    cout.flush();
                }else if( pref< prefixes.begin() ){
                    cout << "++++++++++++++++++++++++++failed category:" << value <<endl;
                }

                newId->category=pref - prefixes.begin();

                cout << "id:" << newId->id << endl;
                cout.flush();


                if (newId->id > maxId)
                    maxId = newId->id;
                newNode->id = newId;
            }
            else if(field=="Location"){
                newNode->place = new UnrealPlace((char*) value.c_str());

                if (newNode->place->x<left)
                    left=newNode->place->x;
                else if(newNode->place->x>right)
                    right=newNode->place->x;

                if (newNode->place->y<top)
                    top=newNode->place->y;
                else if(newNode->place->y>bottom)
                    bottom=newNode->place->y;

                if (((UnrealPlace*) newNode->place)->z<zMin)
                    zMin=((UnrealPlace*) newNode->place)->z;
                else if( ((UnrealPlace*) newNode->place)->z>zMax)
                    zMax=((UnrealPlace*) newNode->place)->z;
            }
        }
        goto newLine;
    }


    int prefCount = prefixes.size();

    lookupTable = (Node***) malloc(prefCount * sizeof(Node**));
    for (int i=0; i<prefCount; i++){
        int cnt = prefixCounts[i];
        lookupTable[i]=(Node**) malloc(cnt * sizeof(Node*));
        cout<< i << " = " << cnt<< endl;
    }

    vector<Node>::iterator iter;
    for(iter=nodes.begin(); iter<nodes.end();iter++){
        cout << "added " << ((UnrealNodeId*) iter->id)->category << ":" <<iter->id->id <<endl;
        lookupTable[((UnrealNodeId*) iter->id) -> category][iter->id->id] = & (*iter);
    }
    //(cout<< "done adding\n").flush();
}


void UnrealMap::readInputSNGP(UnrealStream& input, Node* node){
//    Mutex::Lock lock(mutex);
    string mess;
    cout << "handle SNGP" <<endl;
    cout.flush();
    for(;;){
        mess=input.nextMessage();
        if(mess == "ENGP") return;
        if(mess =="INGP"){

            UnrealPath* newPath = new UnrealPath(((UnrealNodeId*) node->id)->category, node->id->id);
            for (int i=0; i<input.fieldCount();i++){
                string field = input[i].first;
                string value = input[i].second;


                if(field == "Id"){
					newPath->name=value;
                    int numPos = value.find_last_not_of("0123456789")+1;
                    newPath->toId.id=atoi(value.c_str()+numPos);
                    value.resize(numPos);
                    vector<string>::iterator pref = find(prefixes.begin(), prefixes.end(), value);
                    if(prefixes.size()==0 || pref==prefixes.end()){
                        prefixes.push_back(value);
                        prefixCounts.push_back(0);
                        pref=prefixes.end()-1;
                        cout << "############################ new category:" << value <<endl;
                    }else if( pref< prefixes.begin() ){
                        cout << "++++++++++++++++++++++++++failed category:" << value <<endl;
                    }
                    newPath->toId.category = pref - prefixes.begin();
                }
                else if(field == "Flags")
                    newPath->flags = atoi(value.c_str());
                else if(field == "CollisionR")
                    newPath->collisionR = atoi(value.c_str());
                else if(field == "CollisionH")
                    newPath->collisionH = atoi(value.c_str());
            }
            node->outgoing.push_back(newPath);
        }
        else return; //shouldn't happen
    }

}
void UnrealMap::finalize(){

    Mutex::Lock lock(mutex);
    cout << "finalizing\n";
    vector<Node>::iterator iter;
    for(iter=nodes.begin(); iter<nodes.end(); iter++){//loop all nodes
        vector<Path*>::iterator pathiter;
        for (pathiter=iter->outgoing.begin(); pathiter < iter->outgoing.end(); pathiter++){
            ((UnrealPath*)(*pathiter))->to=
            &(*
                lookupTable[((UnrealPath*)(*pathiter))->toId.category][((UnrealPath*)(*pathiter))->toId.id]
            );//sorry for the long line of code. It looks up the destination node and saves it as a pointer
            (*pathiter)->from=
            &(*
                lookupTable[((UnrealPath*)(*pathiter))->fromId.category][((UnrealPath*)(*pathiter))->fromId.id]
            );


        }
    }
	RouteMap::finalize();
}

void UnrealMap::invalidate(){

	this->prefixCounts.clear();
	this->prefixes.clear();

	RouteMap::invalidate();
}


UnrealMap::~UnrealMap(){
    //dtor
}
